/* USER CODE BEGIN Header */
/**
 ******************************************************************************
 * File Name          :
 * Description        :
 ******************************************************************************
 * @attention
 *
 *
 ******************************************************************************
 */
/* USER CODE END Header */

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "main.h"
/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */

/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN Variables */
SysStruct g_sys;

/* USER CODE END Variables */

/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN FunctionPrototypes */

/* USER CODE END FunctionPrototypes */

/* Private application code --------------------------------------------------*/
/* USER CODE BEGIN Application */
/**
 * @brief  initialize  peripheral about driver of motor
 * @note
 * @param
 * @retval
 */
void Peripheral_Init_Task(void)
{
	// Encoder Init
	uint16_t error_count = 0;
	while (ICMU_Data[IC_MU_INNER].active_state != ACTIVATE_SUCCESS_FLAG)
	{
		ICMU_Data[IC_MU_INNER].active_state = IC_MU_Active(IC_MU_INNER);
		error_count++;
		if (error_count > 1000)
		{
			// error
			error_count = 0;
			break;
		}
	}
	error_count = 0;
	while (ICMU_Data[IC_MU_OUTER].active_state != ACTIVATE_SUCCESS_FLAG)
	{
		ICMU_Data[IC_MU_OUTER].active_state = IC_MU_Active(IC_MU_OUTER);
		error_count++;
		if (error_count > 1000)
		{
			// error
			error_count = 0;
			break;
		}
	}

	// TIM Init
	LL_TIM_EnableIT_UPDATE(TIM1);
	LL_TIM_OC_EnablePreload(TIM1, LL_TIM_CHANNEL_CH1);
	LL_TIM_OC_EnablePreload(TIM1, LL_TIM_CHANNEL_CH2);
	LL_TIM_OC_EnablePreload(TIM1, LL_TIM_CHANNEL_CH3);
	LL_TIM_OC_EnablePreload(TIM1, LL_TIM_CHANNEL_CH4);
	LL_TIM_CC_DisableChannel(TIM1, ((uint16_t)(LL_TIM_CHANNEL_CH1 | LL_TIM_CHANNEL_CH1N |
											   LL_TIM_CHANNEL_CH2 | LL_TIM_CHANNEL_CH2N |
											   LL_TIM_CHANNEL_CH3 | LL_TIM_CHANNEL_CH3N)));
	LL_TIM_CC_EnableChannel(TIM1, ((uint16_t)LL_TIM_CHANNEL_CH4));

	LL_TIM_EnableAllOutputs(TIM1);
	LL_TIM_EnableCounter(TIM1);

	// ADC Init
	LL_ADC_ClearFlag_JEOS(ADC1);
	LL_ADC_EnableIT_JEOS(ADC1);
	LL_ADC_ClearFlag_JEOC(ADC1);
	LL_ADC_EnableIT_JEOC(ADC1);
	LL_ADC_INJ_StartConversion(ADC1);

	FocOrder_DisableDriver();
	// trajectory calculate parameters
	Trajectory_Parameters_Reset();
}
/*------------- motion control function -------------------------*/
volatile float SetVelocity = 0.0f;
volatile float SetPosition = 0.0f;
static float dError = 0.0f, IQ = 0.0f, ID = 0.0f;
static void torque_control_task(float set)
{
	if (set > g_sys.RW.UserPara.Para.IQLimit)
		set = g_sys.RW.UserPara.Para.IQLimit;
	else if (set < -g_sys.RW.UserPara.Para.IQLimit)
		set = -g_sys.RW.UserPara.Para.IQLimit;

	FocOrder_DQConfig(set, g_sys.Para.SetID);
	foc_commutate(MotorEncoder.elec_angle); // speed 6.64us
}
// optimization 2 speed 2.2us + 6.64us
static void speed_control_task(float set)
{
	// static uint8_t count = 0;
	// if (count >= SPEED_CLPSE_COUNT)
	// {
	dError = set - g_sys.Para.ReadVelocity;
	IQ = PID_Controller(&PIDControl_V, dError);
	ID = 0.0f;
	IQ /= SPEED_PID_GAIN;

	torque_control_task(IQ);
	// 	count = 0;
	// }
	// count++;
}
// optimization 2 speed 2.98us + 6.64us
static void position_control_task(float set)
{
	// static uint8_t count = 0;
	// if (count >= POSITION_CLPSE_COUNT)
	// {
	dError = set - g_sys.Para.ReadPosition;
	dError *= 16384;
	g_sys.Para.SetVelocity = PID_Controller(&PIDControl_P, dError);
	if (fabs(g_sys.Para.SetVelocity) < 10)
		g_sys.Para.SetVelocity = 0;
	speed_control_task(g_sys.Para.SetVelocity);
	// 	count = 0;
	// }
	// count++;
}

static void speed_profile_control_task(void)
{
	speed_control_task(Ramp_Speed(g_sys.Para.SetVelocity));
}

static void position_profile_control_task(void)
{
	position_control_task(Trajectory_Move());
}

/**
 * @brief  Clarke-Park Transform,current closed loop,the control frequency is 20Khz
 * @note   We know that by testing,system takes 19.658us to complete a cycle of torque,
 * @param
 * @retval
 */
static float test_theta_ref = 0;
void FocRun_Task(void)
{
#ifdef DEBUG_EVENT_RECORDER_20K
	EventStartA(0);
#endif
	// first you must save data of current
	FocOrder_CurrentUpdate(); // optimization 2 speed 1.26us
	// acquire position data
	encoder_routine(); // optimization 2 speed 11.7us

	switch (g_sys.State)
	{
	case STATE_START:
		g_sys.State = STATE_RUN;
		if (g_sys.RW.UserPara.Para.ProtocolType == ProtocolMinta) // return
			Minta_CAN_MessageSend(S_ON_OFF, 0x01, 0, 1);
		break;
	case STATE_RUN:
		switch (g_sys.Mode)
		{
		case MODE_CALI:
			// run encoder calibration
			motor_encoder_calibration(&MotorEncoder, &MotorController, &Calibration);
			break;
		case MODE_TORQUE:
			torque_control_task(g_sys.Para.SetIQ);
			break;
		case MODE_SPEED:
			speed_control_task(g_sys.Para.SetVelocity);
			break;
		case MODE_POSITION:
			position_control_task(g_sys.Para.SetPosition);
			break;
		case MODE_TPOSITION:
			position_profile_control_task();
			break;
		case MODE_TSPEED:
			speed_profile_control_task();
			break;
		case MODE_HOMING:
			FocOrder_DQConfig(0.0f, 0.0f);
			foc_commutate(MotorEncoder.elec_angle); // speed 5.46us
			break;

		case MODE_TEST:
			test_theta_ref += 0.001;
			foc_commutate_openclosecurrent(test_theta_ref);
		default:

			break;
		}
		break;
	default:
		// FocOrder_CurrentUpdate();
		break;
	}
	// 20K refresh monitor
	HFMonitor_Task();

#ifdef DEBUG_EVENT_RECORDER_20K
	EventStopA(0);
#endif
}

/**
 * @brief  Motor running state machine,the control frequency is 1Khz
 * @note
 * @param
 * @retval
 */
void FocState_Task(void)
{
	// Monitor_Task();
	switch (g_sys.State)
	{
	case STATE_IDLE:
		//g_sys.State = STATE_INIT;
		break;
	case STATE_INIT:
		FocOrder_EnableDriver();
		Trajectory_Parameters_Reset();
		g_sys.State = STATE_CALI;
		break;
	case STATE_CALI:
		// wait current calibration
		if (FocOrder_CaliOffsetCurrent(&MotorController))
		{
			g_sys.State = STATE_START;
		}
		break;

	case STATE_START:

		break;
	case STATE_RUN:
	{
		switch (g_sys.Mode)
		{
		case MODE_CALI:

			break;

		case MODE_TORQUE:

			break;
		case MODE_SPEED:

			break;
		case MODE_TSPEED:

			break;
		case MODE_TPOSITION:
		case MODE_POSITION:

			break;
		case MODE_HOMING:

			break;

		default:

			break;
		}
	}
	break;
	case STATE_STOP:
		FocOrder_DisableDriver();
		g_sys.State = STATE_IDLE;
		if (g_sys.RW.UserPara.Para.ProtocolType == ProtocolMinta) // return
			Minta_CAN_MessageSend(S_ON_OFF, 0x01, 0, 1);
		break;
	case STATE_ERROR:
		FocOrder_DisableDriver();

		break;

	default:
		break;
	}
}
// high frequency refresh parameter
void HFMonitor_Task(void)
{
	// acqire postion
	g_sys.Para.ReadPosition = MotorEncoder.angle_multi;
	// acqire volecity
	g_sys.Para.ReadVelocity = MotorEncoder.estimate_velocity;
	g_sys.Para.ReadIQ = MotorController.i_q / Amp_Num_SCALE;
	g_sys.Para.ReadID = MotorController.i_d / Amp_Num_SCALE;
}

/* USER CODE END Application */
